/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package org.electrode.base;

import org.electrode.common.CompoundSystem;
import org.electrode.system.Camera;
import org.electrode.system.Climber;
import org.electrode.system.Drive;
import org.electrode.system.Shooter;

import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.networktables.NetworkTable;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class Robot extends IterativeRobot {
	private CompoundSystem all;
	private XboxJoystick[] joysticks;

	/**
	 * This function is run when the robot is first started up and should be
	 * used for any initialization code.
	 */
	public void robotInit() {
		joysticks = new XboxJoystick[] { createJoystick(1), createJoystick(2) };

		all = new CompoundSystem("All");
		// all.addSystem(new Camera(joysticks[1]));
		all.addSystem(new Drive(joysticks[0]));
		all.addSystem(new Shooter(joysticks[1]));
		// all.addSystem(new Climber(joysticks[1]));
		; // all.addSystem(new Research());
		all.init();
	}

	private XboxJoystick createJoystick(int port) {
		XboxJoystick control = new XboxJoystick(port);
		NetworkTable table = NetworkTable.getTable("Inputs");
		control.initTable(table.getSubTable("xbox-" + port));
		return control;
	}

	public void autonomousInit() {
		all.enableAuto();
	}

	public void disabledInit() {
		all.disable();
	}

	public void disabledPeriodic() {
		updateJoystickNetworkTables();
		all.disable();
	}

	/**
	 * This function is called periodically during autonomous
	 */
	public void autonomousPeriodic() {
		all.periodicAuto();
	}

	public void teleopInit() {
		all.enableTeleop();
	}

	/**
	 * This function is called periodically during operator control
	 */
	public void teleopPeriodic() {
		updateJoystickNetworkTables();
		all.periodicTeleop();
	}

	public void testInit() {
		all.enableTest();
	}

	/**
	 * This function is called periodically during test mode
	 */
	public void testPeriodic() {
		updateJoystickNetworkTables();
		all.periodicTest();
	}

	private void updateJoystickNetworkTables() {
		for (int i = 0; i < joysticks.length; i++) {
			joysticks[i].updateTable();
		}
	}

}
